#!/usr/bin/env python3
import math
import hardware
import time
import odrive
import KociembaTrans
import kociemba
arm_acting_time_90 = 0.12#0.1
arm_acting_time_180 = 0.17#0.18
hand_acting_time = 0.14
slower_time = 0
def _Acting(Action):
    global slower_time
    if (Action[0] == 'slower'):
       hardware.slower_Arms()
       # time.sleep(0.02)
       slower_time = 0
    elif (Action[0] == 'faster'):
       hardware.faster_Arms()
       # time.sleep(0.02)
       slower_time = 0
       # slower_time = 0.06
    elif (Action[0] == 'RW'):
       # print(hardware.odrv_up.axis0.trap_traj.config.accel_limit)
       # print(hardware.odrv_up.axis1.trap_traj.config.accel_limit)
       # print(hardware.odrv_up.axis0.trap_traj.config.decel_limit)
       # print(hardware.odrv_up.axis1.trap_traj.config.decel_limit)
       # print('')
       if (Action[1] == 90):
           hardware.arm0_rotate90_clockwise()
           time.sleep(arm_acting_time_90-slower_time)
       elif (Action[1] == -90):
           hardware.arm0_rotate90_anticlockwise()
           time.sleep(arm_acting_time_90-slower_time)
       elif (Action[1] == 180): 
           hardware.arm0_rotate180_clockwise()
           time.sleep(arm_acting_time_180-slower_time)
       
    elif (Action[0] == 'LW'):
       # print(hardware.odrv_up.axis0.trap_traj.config.accel_limit)
       # print(hardware.odrv_up.axis1.trap_traj.config.accel_limit)
       # print(hardware.odrv_up.axis0.trap_traj.config.decel_limit)
       # print(hardware.odrv_up.axis1.trap_traj.config.decel_limit)
       # print('')
       if (Action[1] == 90):
           hardware.arm1_rotate90_clockwise()
           time.sleep(arm_acting_time_90-slower_time)
       elif (Action[1] == -90):
           hardware.arm1_rotate90_anticlockwise()
           time.sleep(arm_acting_time_90-slower_time)
       elif (Action[1] == 180): 
           hardware.arm1_rotate180_clockwise()
           time.sleep(arm_acting_time_180-slower_time)
       
    elif (Action[0] == 'RH'):
       if (Action[1] == 'open'):
           hardware.loosen_hand0()
       if (Action[1] == 'close'):
           hardware.catch_hand0()
       time.sleep(hand_acting_time)
    elif (Action[0] == 'LH'):
       if (Action[1] == 'open'):
           hardware.loosen_hand1()
       if (Action[1] == 'close'):
           hardware.catch_hand1()
       time.sleep(hand_acting_time)
  
def Moving(command): #1 means clockwise 90 degree
                     #2 means 180 degree
                     #3 means anticlockwise 90 degree
                     #RW RH means motor0 of each odrive
                     #LW LH means motor1 of each odrive
       
    dict ={   'D1': [('LW',-90), ('LH', 'open'),('LW',90),('LH', 'close')],
              'D2': [('LW',180)],
              'D3': [('LW',90), ('LH', 'open'),('LW',-90),('LH', 'close')],
              #D Coordinate has no change
              'B1': [('RW',-90), ('RH', 'open'),('RW',90),('RH', 'close')],
              'B2': [('RW',180)],
              'B3': [('RW',90), ('RH', 'open'),('RW',-90),('RH', 'close')],
              #B Coordinate has no change
              'F1': [('RH', 'open'),('slower',' '),('LW',180),('RH', 'close'),('faster',' '),#0.4
                     ('RW',-90), ('RH', 'open'),('RW',90),('RH', 'close')],
              'F2': [('RH', 'open'),('slower',' '),('LW',180),('RH', 'close'),('faster',' '),
                     ('RW',180)],
              'F3': [('RH', 'open'),('slower',' '),('LW',180),('RH', 'close'),('faster',' '),
                     ('RW',90), ('RH', 'open'),('RW',-90),('RH', 'close')],
              #F F->B B->F L->R R->L
              'U1': [('LH', 'open'),('slower',' '),('RW',180),('LH', 'close'),('faster',' '),#0.4
                     ('LW',-90), ('LH', 'open'),('LW',90),('LH', 'close')],
              'U2': [('LH', 'open'),('slower',' '),('RW',180),('LH', 'close'),('faster',' '),
                     ('LW',180)],
              'U3': [('LH', 'open'),('slower',' '),('RW',180),('LH', 'close'),('faster',' '),
                     ('LW',90), ('LH', 'open'),('LW',-90),('LH', 'close')],
              #U U->D D->U L->R R->L
              'L1': [('LH', 'open'),('slower',' '),('RW',-90),('LH', 'close'),('RH', 'open'),('RW',90),('RH', 'close'),('faster',' '),
                     ('LW',-90), ('LH', 'open'),('LW',90),('LH', 'close')],
              'L2': [('LH', 'open'),('slower',' '),('RW',-90),('LH', 'close'),('RH', 'open'),('RW',90),('RH', 'close'),('faster',' '),
                     ('LW',180)],
              'L3': [('LH', 'open'),('slower',' '),('RW',-90),('LH', 'close'),('RH', 'open'),('RW',90),('RH', 'close'),('faster',' '),
                     ('LW',90), ('LH', 'open'),('LW',-90),('LH', 'close')],
              #L L->D D->R R->U U->L
              'R1': [('LH', 'open'),('slower',' '),('RW',90),('LH', 'close'),('RH', 'open'),('RW',-90),('RH', 'close'),('faster',' '),
                     ('LW',-90), ('LH', 'open'),('LW',90),('LH', 'close')],
              'R2': [('LH', 'open'),('slower',' '),('RW',90),('LH', 'close'),('RH', 'open'),('RW',-90),('RH', 'close'),('faster',' '),
                     ('LW',180)],
              'R3': [('LH', 'open'),('slower',' '),('RW',90),('LH', 'close'),('RH', 'open'),('RW',-90),('RH', 'close'),('faster',' '),
                     ('LW',90), ('LH', 'open'),('LW',-90),('LH', 'close')],
              #R R->D D->L L->U U->R

              'l1': [('RH', 'open'),('slower',' '),('LW',90),('RH', 'close'),('LH', 'open'),('LW',-90),('LH', 'close'),('faster',' '),
                     ('RW',-90), ('RH', 'open'),('RW',90),('RH', 'close')],
              'l2': [('RH', 'open'),('slower',' '),('LW',90),('RH', 'close'),('LH', 'open'),('LW',-90),('LH', 'close'),('faster',' '),
                     ('RW',180)],
              'l3': [('RH', 'open'),('slower',' '),('LW',90),('RH', 'close'),('LH', 'open'),('LW',-90),('LH', 'close'),('faster',' '),
                     ('RW',90), ('RH', 'open'),('RW',-90),('RH', 'close')],
              #l L->B B->R R->F F->L
              'r1': [('RH', 'open'),('slower',' '),('LW',-90),('RH', 'close'),('LH', 'open'),('LW',90),('LH', 'close'),('faster',' '),
                     ('RW',-90), ('RH', 'open'),('RW',90),('RH', 'close')],
              'r2': [('RH', 'open'),('slower',' '),('LW',-90),('RH', 'close'),('LH', 'open'),('LW',90),('LH', 'close'),('faster',' '),
                     ('RW',180)],
              'r3': [('RH', 'open'),('slower',' '),('LW',-90),('RH', 'close'),('LH', 'open'),('LW',90),('LH', 'close'),('faster',' '),
                     ('RW',90), ('RH', 'open'),('RW',-90),('RH', 'close')],
              #r R->B B->L L->F F->R
            }
    for item in dict[command]:
        _Acting(item)



def Solution_Optimize(Solution): #1 means clockwise 90 degree
                     #2 means 180 degree
                     #3 means anticlockwise 90 degree
                     #RW RH means motor0 of each odrive
                     #LW LH means motor1 of each odrive
       
    dict ={   'D1': [('LW',-90), ('LH', 'open'),('LW',90),('LH', 'close')],
              'D2': [('LW',180)],
              'D3': [('LW',90), ('LH', 'open'),('LW',-90),('LH', 'close')],
              #D Coordinate has no change
              'B1': [('RW',-90), ('RH', 'open'),('RW',90),('RH', 'close')],
              'B2': [('RW',180)],
              'B3': [('RW',90), ('RH', 'open'),('RW',-90),('RH', 'close')],
              #B Coordinate has no change
              'F1': [('RH', 'open'),('slower',' '),('LW',180),('RH', 'close'),('faster',' '),
                     ('RW',-90), ('RH', 'open'),('RW',90),('RH', 'close')],
              'F2': [('RH', 'open'),('slower',' '),('LW',180),('RH', 'close'),('faster',' '),
                     ('RW',180)],
              'F3': [('RH', 'open'),('slower',' '),('LW',180),('RH', 'close'),('faster',' '),
                     ('RW',90), ('RH', 'open'),('RW',-90),('RH', 'close')],
              #F F->B B->F L->R R->L
              'U1': [('LH', 'open'),('slower',' '),('RW',180),('LH', 'close'),('faster',' '),
                     ('LW',-90), ('LH', 'open'),('LW',90),('LH', 'close')],
              'U2': [('LH', 'open'),('slower',' '),('RW',180),('LH', 'close'),('faster',' '),
                     ('LW',180)],
              'U3': [('LH', 'open'),('slower',' '),('RW',180),('LH', 'close'),('faster',' '),
                     ('LW',90), ('LH', 'open'),('LW',-90),('LH', 'close')],
              #U U->D D->U L->R R->L
              'L1': [('LH', 'open'),('slower',' '),('RW',-90),('LH', 'close'),('RH', 'open'),('RW',90),('RH', 'close'),('faster',' '),
                     ('LW',-90), ('LH', 'open'),('LW',90),('LH', 'close')],
              'L2': [('LH', 'open'),('slower',' '),('RW',-90),('LH', 'close'),('RH', 'open'),('RW',90),('RH', 'close'),('faster',' '),
                     ('LW',180)],
              'L3': [('LH', 'open'),('slower',' '),('RW',-90),('LH', 'close'),('RH', 'open'),('RW',90),('RH', 'close'),('faster',' '),
                     ('LW',90), ('LH', 'open'),('LW',-90),('LH', 'close')],
              #L L->D D->R R->U U->L
              'R1': [('LH', 'open'),('slower',' '),('RW',90),('LH', 'close'),('RH', 'open'),('RW',-90),('RH', 'close'),('faster',' '),
                     ('LW',-90), ('LH', 'open'),('LW',90),('LH', 'close')],
              'R2': [('LH', 'open'),('slower',' '),('RW',90),('LH', 'close'),('RH', 'open'),('RW',-90),('RH', 'close'),('faster',' '),
                     ('LW',180)],
              'R3': [('LH', 'open'),('slower',' '),('RW',90),('LH', 'close'),('RH', 'open'),('RW',-90),('RH', 'close'),('faster',' '),
                     ('LW',90), ('LH', 'open'),('LW',-90),('LH', 'close')],
              #R R->D D->L L->U U->R

              'l1': [('RH', 'open'),('slower',' '),('LW',90),('RH', 'close'),('LH', 'open'),('LW',-90),('LH', 'close'),('faster',' '),
                     ('RW',-90), ('RH', 'open'),('RW',90),('RH', 'close')],
              'l2': [('RH', 'open'),('slower',' '),('LW',90),('RH', 'close'),('LH', 'open'),('LW',-90),('LH', 'close'),('faster',' '),
                     ('RW',180)],
              'l3': [('RH', 'open'),('slower',' '),('LW',90),('RH', 'close'),('LH', 'open'),('LW',-90),('LH', 'close'),('faster',' '),
                     ('RW',90), ('RH', 'open'),('RW',-90),('RH', 'close')],
              #l L->B B->R R->F F->L
              'r1': [('RH', 'open'),('slower',' '),('LW',-90),('RH', 'close'),('LH', 'open'),('LW',90),('LH', 'close'),('faster',' '),
                     ('RW',-90), ('RH', 'open'),('RW',90),('RH', 'close')],
              'r2': [('RH', 'open'),('slower',' '),('LW',-90),('RH', 'close'),('LH', 'open'),('LW',90),('LH', 'close'),('faster',' '),
                     ('RW',180)],
              'r3': [('RH', 'open'),('slower',' '),('LW',-90),('RH', 'close'),('LH', 'open'),('LW',90),('LH', 'close'),('faster',' '),
                     ('RW',90), ('RH', 'open'),('RW',-90),('RH', 'close')],
              #r R->B B->L L->F F->R
            }
    Step_list = []
    for Commend in Solution:
       for step in dict[Commend]:
           Step_list.append(step)
    Step_list.append(('None','None'))
    Step_list_Optimized = []
    JumpFlag = 0
    cnt = 0
    for step_num in range(len(Step_list)-1):
       if (JumpFlag == 1):
           JumpFlag = 0
           continue
       if (Step_list[step_num][0] == Step_list[step_num+1][0] and Step_list[step_num][1] == 'close'):
           cnt = cnt+1
           JumpFlag = 1
           continue
       else:
           Step_list_Optimized.append(Step_list[step_num])
#     print(Step_list)
#     print(Step_list_Optimized)
    print(cnt)
    time_begin = time.time()
    for step in Step_list_Optimized:
        _Acting(step)
    print('time = ' + str(time.time()-time_begin))
if __name__ == '__main__':
    Solution = kociemba.solve('FLBUULFFLFDURRDBUBUUDDFFBRDDBLRDRFLLRLRULFUDRRBDBBBUFL')
    realsolution = KociembaTrans.TransToRealSolution(Solution)
    Solution_Optimize(realsolution)
#     print(solution)